import lejos.nxt.*;
import lejos.robotics.navigation.*;

/**
  * Robot that stops if it hits something before it completes its travel.
  * taken from: http://lejos.sourceforge.net/nxt/nxj/tutorial/WheeledVehicles/WheeledVehicles.htm
  * Changed to use TachoPilot, by Michael Torp Kaalund, 9 nov 2010
  */

public class TravelTest implements ButtonListener {
	TachoPilot pilot;
	SimpleNavigator Navi;
//	TouchSensor bump = new TouchSensor(SensorPort.S1);
	private boolean ButtonPressed = false;	


	public void buttonPressed(Button b) {
		ButtonPressed = true;
	}

	public void buttonReleased(Button b) {
	}

	public void go()
	{
		Navi.goTo(0.0f,20.0f);
		Navi.goTo(20.0f, 20.0f);
		Navi.goTo(20.0f, 0.0f);
		Navi.goTo(0.0f,0.0f);

	}

	public static void main(String[] args)
	{
		/*
			Bette hjul : 2.1 cm eller 2.5 cm eller 2.8 cm eller 3 cm
			Bette tandhjul : 12 tænder
			Stor Tandhjul  : 40 tænder
			indre track    : 11 cm

			12/40 * 30 mm = 9 
			9  + 11 = 20
			
		*/
		TravelTest traveler = new TravelTest();
		traveler.pilot = new TachoPilot(9.0f, 15.7f, Motor.A, Motor.C);
		//traveler.pilot = new TachoPilot(3.3f, 10.7f, Motor.A, Motor.C);
		traveler.Navi = new SimpleNavigator(traveler.pilot);
		traveler.Navi.setPosition(0,0,90);
		//new TachoPilot(9.0f, 19.0f, Motor.A, Motor.C);
		traveler.go();
	}


}
